/*!
 * @file mpu9250_lpspi_edma.c
 * @author suyong_yq@126.com
 */

#include "mpu9250_lpspi_edma.h"
#include "mpu9250_reg.h"
#include "hal_gpio.h"

/* 内部变量定义 */
LPSPI_EDMA_MasterXferHandler_T gAppLpspiEdmaXferHandle;
LPSPI_EDMA_MasterXferHandler_T * volatile gAppLpspiEdmaXferHandler = &gAppLpspiEdmaXferHandle;

LPSPI_EDMA_HwDev_T gAppLpspiEdmaHwDevStruct =
{
    .LpspiBase = LPSPI0,
    .EdmaLpspiTxChnIdx = 0U,
    .EdmaLpspiTxTriggerSource = eDMAMUX_TriggerSource_LPSPI0_Tx,
    .EdmaLpspiRxChnIdx = 1U,
    .EdmaLpspiRxTriggerSource = eDMAMUX_TriggerSource_LPSPI0_Rx,
    .EdmaRxChnIRQn = DMA1_IRQn /* 对应EdmaLpspiRxChnIdx */
};

LPSPI_MasterConfig_T gAppLpspiMasterConfigStruct =
{
    .enCSHigh = false,
    .TxFIFOWatermark = 1U,
    .RxFIFOWatermark = 0U
};

/* 内部函数声明 */
static void LPSPI_Configuration(void);
static void EDMA_Configuration(void);

volatile bool gAppEdmaRxDoneFlag = false;
static void App_EdmaPollingDoneCallback(void)
{
    gAppEdmaRxDoneFlag = true;
}

bool MPU9250_Init(void)
{
    LPSPI_Configuration();
    EDMA_Configuration();

    /* 初始化LPSPI_EDMA传输 */
    LPSPI_EDMA_MasterInit(gAppLpspiEdmaXferHandler,
        &gAppLpspiEdmaHwDevStruct,
        &gAppLpspiMasterConfigStruct );

    return true;
}

/* 发送WHO_AM_I命令 */
bool MPU9250_Probe(void)
{
    uint8_t cmdWhoAmI[2] = {
        MPU9250_SPI_READ_REG_CMD(MPU9250_WHOAMI),
        0xFF
    };
    uint8_t edmaRxBuf[2];

    GPIO_SetPinLogic(GPIOB, 0U, false);
    gAppEdmaRxDoneFlag = false;
    LPSPI_EDMA_StartMasterXfer(
            gAppLpspiEdmaXferHandler,
            cmdWhoAmI,
            edmaRxBuf,
            2U,
            App_EdmaPollingDoneCallback );
    while (!gAppEdmaRxDoneFlag)
    {}
    if (0x71 != edmaRxBuf[1]) /* MPU9250的设备ID为0x71 */
    {
        return false;
    }
    return true;
}

#if 0
//初始化MPU9250，根据需要请参考pdf进行修改************************
void Init_MPU9250(void)
{
/*
   Single_Write(GYRO_ADDRESS,PWR_M, 0x80);   //
   Single_Write(GYRO_ADDRESS,SMPL, 0x07);    //
   Single_Write(GYRO_ADDRESS,DLPF, 0x1E);    //±2000°
   Single_Write(GYRO_ADDRESS,INT_C, 0x00 );  //
   Single_Write(GYRO_ADDRESS,PWR_M, 0x00);   //
*/
    Single_Write(GYRO_ADDRESS,PWR_MGMT_1, 0x00);	//解除休眠状态
    Single_Write(GYRO_ADDRESS,SMPLRT_DIV, 0x07);
    Single_Write(GYRO_ADDRESS,CONFIG, 0x06);
    Single_Write(GYRO_ADDRESS,GYRO_CONFIG, 0x18);
    Single_Write(GYRO_ADDRESS,ACCEL_CONFIG, 0x01);
    //----------------
    //	Single_Write(GYRO_ADDRESS,0x6A,0x00);//close Master Mode

}
#endif

const uint8_t Mpu9250CmdTxReset[] = {MPU9250_SPI_WRITE_REG_CMD(MPU9250_PWR_MGMT_1), MPU9250_PWR_MGMT_1_H_RESET_MASK};
const uint8_t Mpu9250CmdSelClkSrc[] = {MPU9250_SPI_WRITE_REG_CMD(MPU9250_PWR_MGMT_1), MPU9250_PWR_MGMT_1_CLKSEL(0x03)}; /* PLL_GYRO_Z */
const uint8_t Mpu9250CmdEnableGyroAccSensor[] = {MPU9250_SPI_WRITE_REG_CMD(MPU9250_PWR_MGMT_2), 0x0}; /* Enable Gyro & Acc sensor. */
const uint8_t Mpu9250CmdSetSampleRate[] = {MPU9250_SPI_WRITE_REG_CMD(MPU9250_SMPLRT_DIV), 0x19}; /* 陀螺仪采样率 */
const uint8_t Mpu9250CmdSetGyroConfig[] = {MPU9250_SPI_WRITE_REG_CMD(MPU9250_GYRO_CONFIG), MPU9250_GYRO_CONFIG_GYRO_FS_SEL(0x3)}; /* 2000 dps. */
const uint8_t Mpu9250CmdSetAccConfig[] = {MPU9250_SPI_WRITE_REG_CMD(MPU9250_ACCEL_CONFIG), MPU9250_ACCEL_CONFIG_ACCEL_FS_SEL(0x0), 0x0}; /* +- 2G. */

void MPU9250_Setup(void)
{
    volatile uint32_t i;
    //uint8_t edmaRxBuf[10]; /* 接收缓冲区 */

    /* 复位设备 */
    GPIO_SetPinLogic(GPIOB, 0U, false); /* CSn = 0. */
    gAppEdmaRxDoneFlag = false;
    LPSPI_EDMA_StartMasterXfer(
            gAppLpspiEdmaXferHandler,
            (uint8_t *)Mpu9250CmdTxReset,
            NULL,
            sizeof(Mpu9250CmdTxReset),
            App_EdmaPollingDoneCallback );
    while (!gAppEdmaRxDoneFlag)
    {}
    for (i = 0U; i < 10000U; i++) /* 等待芯片复位过程 */
    {
        __NOP();
    }

    /* 配置设备工作的时钟源 */
    GPIO_SetPinLogic(GPIOB, 0U, false); /* CSn = 0. */
    gAppEdmaRxDoneFlag = false;
    LPSPI_EDMA_StartMasterXfer(
            gAppLpspiEdmaXferHandler,
            (uint8_t *)Mpu9250CmdSelClkSrc,
            NULL,
            sizeof(Mpu9250CmdSelClkSrc),
            App_EdmaPollingDoneCallback );
    while (!gAppEdmaRxDoneFlag)
    {}
    for (i = 0U; i < 1000U; i++) /* 等待时钟源稳定 */
    {
        __NOP();
    }

    /* 使能采集传感器的值 */
    GPIO_SetPinLogic(GPIOB, 0U, false); /* CSn = 0. */
    gAppEdmaRxDoneFlag = false;
    LPSPI_EDMA_StartMasterXfer(
            gAppLpspiEdmaXferHandler,
            (uint8_t *)Mpu9250CmdEnableGyroAccSensor,
            NULL,
            sizeof(Mpu9250CmdEnableGyroAccSensor),
            App_EdmaPollingDoneCallback );
    while (!gAppEdmaRxDoneFlag)
    {}
    for (i = 0U; i < 1000U; i++) /* 等待时钟源稳定 */
    {
        __NOP();
    }

    /* 设定陀螺仪采样率 */
    GPIO_SetPinLogic(GPIOB, 0U, false); /* CSn = 0. */
    gAppEdmaRxDoneFlag = false;
    LPSPI_EDMA_StartMasterXfer(
            gAppLpspiEdmaXferHandler,
            (uint8_t *)Mpu9250CmdSetSampleRate,
            NULL,
            sizeof(Mpu9250CmdSetSampleRate),
            App_EdmaPollingDoneCallback );
    while (!gAppEdmaRxDoneFlag)
    {}
    for (i = 0U; i < 1000U; i++) /* 等待配置生效 */
    {
        __NOP();
    }

    /* 设定Gyro的采样范围 */
    GPIO_SetPinLogic(GPIOB, 0U, false); /* CSn = 0. */
    gAppEdmaRxDoneFlag = false;
    LPSPI_EDMA_StartMasterXfer(
            gAppLpspiEdmaXferHandler,
            (uint8_t *)Mpu9250CmdSetGyroConfig,
            NULL,
            sizeof(Mpu9250CmdSetGyroConfig),
            App_EdmaPollingDoneCallback );
    while (!gAppEdmaRxDoneFlag)
    {}
    for (i = 0U; i < 1000U; i++) /* 等待配置生效 */
    {
        __NOP();
    }

    /* 设定Accel的采样范围 */
    GPIO_SetPinLogic(GPIOB, 0U, false); /* CSn = 0. */
    gAppEdmaRxDoneFlag = false;
    LPSPI_EDMA_StartMasterXfer(
            gAppLpspiEdmaXferHandler,
            (uint8_t *)Mpu9250CmdSetAccConfig,
            NULL,
            sizeof(Mpu9250CmdSetAccConfig),
            App_EdmaPollingDoneCallback );
    while (!gAppEdmaRxDoneFlag)
    {}
    for (i = 0U; i < 1000U; i++) /* 等待配置生效 */
    {
        __NOP();
    }

}

const uint8_t Mpu9250CmdRxSensorData[] = {
    MPU9250_SPI_READ_REG_CMD(MPU9250_ACCEL_XOUT_H),
    0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, /* Accel sensor data. */
    0xFF, 0xFF, /* Temperature sensor data. */
    0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF /* Gyro sensor data. */
};
volatile uint8_t gAppMpu9250SensorDataBuf[15];
void MPU9250_UpdateSensorData(void)
{
    /* 读取传感器数据寄存器的值 */
    GPIO_SetPinLogic(GPIOB, 0U, false); /* CSn = 0. */
    gAppEdmaRxDoneFlag = false;
    LPSPI_EDMA_StartMasterXfer(
            gAppLpspiEdmaXferHandler,
            (uint8_t *)Mpu9250CmdRxSensorData,
            (uint8_t *)gAppMpu9250SensorDataBuf,
            sizeof(Mpu9250CmdRxSensorData),
            App_EdmaPollingDoneCallback );
    while (!gAppEdmaRxDoneFlag)
    {}
}

void MPU9250_GetSensorData(MPU9250_SensorData_T *sensorDat)
{
#if 0
    sensorDat->AccX = (int16_t)(*(int16_t *)(gAppMpu9250SensorDataBuf+1));
    sensorDat->AccY = (int16_t)(*(int16_t *)(gAppMpu9250SensorDataBuf+3));
    sensorDat->AccZ = (int16_t)(*(int16_t *)(gAppMpu9250SensorDataBuf+5));

    sensorDat->Temperature = (int16_t)(*(int16_t *)(gAppMpu9250SensorDataBuf+7));

    sensorDat->GyrX = (int16_t)(*(int16_t *)(gAppMpu9250SensorDataBuf+9));
    sensorDat->GyrY = (int16_t)(*(int16_t *)(gAppMpu9250SensorDataBuf+11));
    sensorDat->GyrZ = (int16_t)(*(int16_t *)(gAppMpu9250SensorDataBuf+13));
#endif
    sensorDat->AccX = gAppMpu9250SensorDataBuf[1];
    sensorDat->AccX = (sensorDat->AccX << 8) | gAppMpu9250SensorDataBuf[2];
    sensorDat->AccY = gAppMpu9250SensorDataBuf[3];
    sensorDat->AccY = (sensorDat->AccY << 8) | gAppMpu9250SensorDataBuf[4];
    sensorDat->AccZ = gAppMpu9250SensorDataBuf[5];
    sensorDat->AccZ = (sensorDat->AccZ << 8) | gAppMpu9250SensorDataBuf[6];

    sensorDat->Temperature = gAppMpu9250SensorDataBuf[7];
    sensorDat->Temperature = (sensorDat->Temperature << 8) | gAppMpu9250SensorDataBuf[8];

    sensorDat->GyrX = gAppMpu9250SensorDataBuf[9];
    sensorDat->GyrX = (sensorDat->GyrX << 8) | gAppMpu9250SensorDataBuf[10];
    sensorDat->GyrY = gAppMpu9250SensorDataBuf[11];
    sensorDat->GyrY = (sensorDat->GyrY << 8) | gAppMpu9250SensorDataBuf[12];
    sensorDat->GyrZ = gAppMpu9250SensorDataBuf[13];
    sensorDat->GyrZ = (sensorDat->GyrZ << 8) | gAppMpu9250SensorDataBuf[14];
}

#if 0

//******读取MPU9250数据****************************************
void READ_MPU9250_ACCEL(void)
{

   BUF[0]=Single_Read(ACCEL_ADDRESS,ACCEL_XOUT_L);
   BUF[1]=Single_Read(ACCEL_ADDRESS,ACCEL_XOUT_H);
   T_X=	(BUF[1]<<8)|BUF[0];
   T_X/=164; 						   //读取计算X轴数据

   BUF[2]=Single_Read(ACCEL_ADDRESS,ACCEL_YOUT_L);
   BUF[3]=Single_Read(ACCEL_ADDRESS,ACCEL_YOUT_H);
   T_Y=	(BUF[3]<<8)|BUF[2];
   T_Y/=164; 						   //读取计算Y轴数据
   BUF[4]=Single_Read(ACCEL_ADDRESS,ACCEL_ZOUT_L);
   BUF[5]=Single_Read(ACCEL_ADDRESS,ACCEL_ZOUT_H);
   T_Z=	(BUF[5]<<8)|BUF[4];
   T_Z/=164; 					       //读取计算Z轴数据
}

void READ_MPU9250_GYRO(void)
{
   BUF[0]=Single_Read(GYRO_ADDRESS,GYRO_XOUT_L);
   BUF[1]=Single_Read(GYRO_ADDRESS,GYRO_XOUT_H);
   T_X=	(BUF[1]<<8)|BUF[0];
   T_X/=16.4; 						   //读取计算X轴数据

   BUF[2]=Single_Read(GYRO_ADDRESS,GYRO_YOUT_L);
   BUF[3]=Single_Read(GYRO_ADDRESS,GYRO_YOUT_H);
   T_Y=	(BUF[3]<<8)|BUF[2];
   T_Y/=16.4; 						   //读取计算Y轴数据
   BUF[4]=Single_Read(GYRO_ADDRESS,GYRO_ZOUT_L);
   BUF[5]=Single_Read(GYRO_ADDRESS,GYRO_ZOUT_H);
   T_Z=	(BUF[5]<<8)|BUF[4];
   T_Z/=16.4; 					       //读取计算Z轴数据

  // BUF[6]=Single_Read(GYRO_ADDRESS,TEMP_OUT_L);
  // BUF[7]=Single_Read(GYRO_ADDRESS,TEMP_OUT_H);
  // T_T=(BUF[7]<<8)|BUF[6];
  // T_T = 35+ ((double) (T_T + 13200)) / 280;// 读取计算出温度
}


void READ_MPU9250_MAG(void)
{
   Single_Write(GYRO_ADDRESS,0x37,0x02);//turn on Bypass Mode
   Delayms(10);
   Single_Write(MAG_ADDRESS,0x0A,0x01);
   Delayms(10);
   BUF[0]=Single_Read (MAG_ADDRESS,MAG_XOUT_L);
   BUF[1]=Single_Read (MAG_ADDRESS,MAG_XOUT_H);
   T_X=(BUF[1]<<8)|BUF[0];

   BUF[2]=Single_Read(MAG_ADDRESS,MAG_YOUT_L);
   BUF[3]=Single_Read(MAG_ADDRESS,MAG_YOUT_H);
   T_Y=	(BUF[3]<<8)|BUF[2];
   						   //读取计算Y轴数据

   BUF[4]=Single_Read(MAG_ADDRESS,MAG_ZOUT_L);
   BUF[5]=Single_Read(MAG_ADDRESS,MAG_ZOUT_H);
   T_Z=	(BUF[5]<<8)|BUF[4];
 					       //读取计算Z轴数据
}

#endif

/*---------------------------------------------------------------------------*/

static void LPSPI_Configuration(void)
{
    /* Enable clock. */
    PCC->CLKCFG[PCC_LPSPI0_INDEX] = PCC_CLKCFG_CGC_MASK | PCC_CLKCFG_PCS(6U); /* System PLL. */
    PCC->CLKCFG[PCC_PORTB_INDEX] = PCC_CLKCFG_CGC_MASK;
    PCC->CLKCFG[PCC_PORTE_INDEX] = PCC_CLKCFG_CGC_MASK;

    /* Pin mux. */
    //PORTB->PCR[0] = PORT_PCR_MUX(3); /* LPSPI0_PCS0. */
    /* SPI需要支持连续传输模式，SPI硬件无法在EDMA模式下自动结束传输，因此需要使用GPIO控制 */
    PORTB->PCR[0] = PORT_PCR_MUX(1); /* LPSPI0_PCS0, GPIO */
    PORTB->PCR[1] = PORT_PCR_MUX(3); /* LPSPI0_SOUT. */
    PORTE->PCR[0] = PORT_PCR_MUX(2); /* LPSPI0_SCLK. */
    PORTE->PCR[1] = PORT_PCR_MUX(2); /* LPSPI0_SIN. */

    /* GPIO-CSn. */
    GPIO_SetPinLogic(GPIOB, 0U, true); /* 1. */
    GPIO_SetPinDir(GPIOB, 0U, true); /* Output. */

    /* LPSPI. */
    LPSPI_HAL_Reset(LPSPI0);
    LPSPI_HAL_Enable(LPSPI0, true);
}

static void EDMA_Configuration(void)
{
    /* 开时钟 */
    PCC->CLKCFG[PCC_DMA0_INDEX] = PCC_CLKCFG_CGC_MASK;
    PCC->CLKCFG[PCC_DMAMUX0_INDEX] = PCC_CLKCFG_CGC_MASK;

    /* 复位EDMA模块 */
    EDMA_HAL_ResetEngine();
}

/* DMA1为接收完成中断 */
void DMA1_IRQHandler(void)
{
    GPIO_SetPinLogic(GPIOB, 0U, true);
    LPSPI_EDMA_MasterXferIRQHandler(gAppLpspiEdmaXferHandler);

}
/* EOF. */

